//VERSION 1.0 - Flown on Mini Bexus 26. Sept 2012 to ~10km alt
/*
 * vx_parachute.ino
 *
 * Vexredus parchute control (Please see flowchart image for further explanation.)
 *
 * Global variables accessed:
 *  - vx_g_para_countdown_clock -- Countdown clock (needs to be global for telemetry access)
 *  - vx_g_pos_last_gps_fix_time -- Last time GPS had fix
 *  - vx_g_tele_last_received_time -- Last time uav had contact with ground station.
 *  - vx_g_flightphase -- Current set (global) flightphase
 *  - g_gps -- Current GPS lat, lon, alt // 

'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' remove all access to this and go through sanitized GPS data instead (from vx_pos)
 */

// ************************ SETTINGS **********************************
#define VX_PARA_BOUNDARYBOX_DISTANCE 30000 // in m: Maximum distance from home base --> BOUNDARY BOX -- FLIGHT SETTING
#define VX_PARA_SAFETYBOX_DISTANCE 1000 // in m: Maximum distance from home base to not release parachute --> SAFETY BOX -- FLIGHT SETTING

#define VX_PARA_MIN_ALTITUDE (long)150000 // in cm: minimum altitude to trigger automatic release of parcahute (LOWALTITUDE). -- FLIGHT SETTING

#define VX_PARA_NOGPSFIX_TRIGGER_TIME (long)60*1000 // in milli seconds, time without GPS fix to trigger automatic release of parachute (NOGPSFIX).
#define VX_PARA_NOGROUNDSTATION_TRIGGER_TIME (long)60*1000 // in milli seconds, time without contact with GS to trigger automatic release of parachute (GS).

#define VX_PARA_RELEASE_WARNING_TIME__NOGPSFIX 60 // in seconds, warning time before releasing parachute due to no GPS fix.
#define VX_PARA_RELEASE_WARNING_TIME__LOWALTITUDE 15 // in seconds, warning time before releasing parachute due to low altitude.
#define VX_PARA_RELEASE_WARNING_TIME__BOUNDARYBOX_BELOW_3KM 30 // in seconds, warning time before releasing parachute due to no GPS fix.
#define VX_PARA_RELEASE_WARNING_TIME__BOUNDARYBOX_ABOVE_3KM 60 // in seconds, warning time before releasing parachute due to no GPS fix.

// ACHTUNG DON'T CHANGE THIS!!! HARDCODED IN ATTINY45
#define VX_PARA_ALIVE_TIME 500 // in milli seconds, alive signal half-period time (how often 1->0, 0->1)

#define VX_PARA_PIN_ALIVE 60 // Arduino PIN: A6 (D: 53+7) (pin numbers start at 0)
#define VX_PARA_PIN_RELEASE 61 // Arduino PIN: A7 (D: 53+8)


// ************************ DEFINE LOGIC ******************************
// Reasons for automatic release of parachute
#define VX_PARA_AUTO_RELEASE__BOUNDARYBOX 0 // Outside boundary area
#define VX_PARA_AUTO_RELEASE__NOGPSFIX 1 // Lost GPS fix for a long time
#define VX_PARA_AUTO_RELEASE__LOWALTITUDE 2 // Low altitude

// Reasons to automatically cancel release of parachute
#define VX_PARA_AUTO_CANCEL__NOGPSFIX_GPSFIX 0 // NOGPSFIX -> Got GPS fix again
#define VX_PARA_AUTO_CANCEL__NOGPSFIX_RC 1 // NOGPSFIX -> Got contact with radio control
#define VX_PARA_AUTO_CANCEL__NOGPSFIX_GS 2 // NOGPSFIX -> Got contact with ground station

#define VX_PARA_AUTO_CANCEL__BOUNDARYBOX 3 // Got back into boundary box

#define VX_PARA_AUTO_CANCEL__LOWALTITUDE_RC 4 // Got contact with radio control
#define VX_PARA_AUTO_CANCEL__LOWALTITUDE_GS 5 // Got contact with ground station

// ********************** INTERNAL VARIABLES **************************
// Auto releasing categories
boolean vx_para_auto_releasing__nogpsfix = false;
boolean vx_para_auto_releasing__boundarybox = false;
boolean vx_para_auto_releasing__lowaltitude = false;
boolean vx_para_auto_releasing__telecommand = false;

// For alive signal
long vx_para_alive_timer = 0;
int vx_para_alive_state = 0;

// For release signal
int vx_para_release_state = 0;
boolean vx_para_RELEASING = false;

// Auto release count down clock logic/timer
boolean vx_para_counting_down = false;
long vx_g_para_countdown_clock_timer = 0;

// ************************ PUBLIC FUNCTIONS **************************
void vx_para_setup() {
  pinMode(VX_PARA_PIN_ALIVE, OUTPUT);
  pinMode(VX_PARA_PIN_RELEASE, OUTPUT);
  digitalWrite(VX_PARA_PIN_ALIVE, LOW);
  digitalWrite(VX_PARA_PIN_RELEASE, LOW);
}

void vx_para_loop() {
  vx_para_update_alive();
  //if(vx_g_pos_had_gps_fix == true) vx_para_check();
  vx_para_release_countdown();
  vx_para_update_release();
}

// Send the alive signal to the ATtiny45
void vx_para_update_alive() {
  // Check if it's time to change the alive state pin
  if((millis() - vx_para_alive_timer) > VX_PARA_ALIVE_TIME) {
    // Reset timer
    vx_para_alive_timer = millis();
    
    // Flip the current state
    if(vx_para_alive_state == 0) {
      digitalWrite(VX_PARA_PIN_ALIVE, LOW);
      vx_para_alive_state = 1;
      //Serial.print("DEBUG: vx_para_alive_state = ");
      //Serial.println(vx_para_alive_state);
    }
    else {
      digitalWrite(VX_PARA_PIN_ALIVE, HIGH);
      vx_para_alive_state = 0;
      //Serial.print("DEBUG: vx_para_alive_state = ");
      //Serial.println(vx_para_alive_state);
    }
  }
}

/*void vx_para_update_release() {
  if(vx_para_RELEASING == true) {
    //if(vx_para_released == false) {
      digitalWrite(VX_PARA_PIN_RELEASE, HIGH);
      Serial.println("DEBUG: vx_para_update_release(): VX_PARA_PIN_RELEASE = HIGH");
      //vx_para_released = true;
    //}
  }
  else {
    // Turn off the release pin
    digitalWrite(VX_PARA_PIN_RELEASE, LOW);
    Serial.println("DEBUG: vx_para_update_release(): VX_PARA_PIN_RELEASE = LOW");
    //vx_para_released = false;
  }
}*/

void vx_para_update_release() {
  // RELEASING 
  if((vx_para_RELEASING == true)) { 
    //digitalWrite(VX_PARA_PIN_RELEASE, HIGH);
    //Serial.println("DEBUG: vx_para_update_release(): VX_PARA_PIN_RELEASE = HIGH");
    
    // Blink the release output
    if(vx_para_release_state == 1) {
      vx_para_release_state = 0;
      digitalWrite(VX_PARA_PIN_RELEASE, HIGH);
      //Serial.println("DEBUG: vx_para_update_release(): VX_PARA_PIN_RELEASE = HIGH");
    }
    else {
      vx_para_release_state = 1;
      digitalWrite(VX_PARA_PIN_RELEASE, LOW);
      //Serial.println("DEBUG: vx_para_update_release(): VX_PARA_PIN_RELEASE = LOW");
    }
  }
  // NOT RELEASING TURN OFF
  else {
    // Turn off the release pin
    digitalWrite(VX_PARA_PIN_RELEASE, LOW);
    //Serial.println("DEBUG: vx_para_update_release(): VX_PARA_PIN_RELEASE = LOW");
  }
  
}


// Tic the count down clock
void vx_para_release_countdown() {
  // Check if countdown has been started
  if((vx_para_counting_down == true) && (vx_g_para_countdown_clock != -1)) {
    // Tick down every second
    if((millis() - vx_g_para_countdown_clock_timer) > 1000) {
      vx_g_para_countdown_clock_timer = millis();
      vx_g_para_countdown_clock = vx_g_para_countdown_clock - 1;
    }
  }
  
  // Check if countdown clock has reached 0 --> then RELEASE PARACHUTE!
  if(vx_g_para_countdown_clock == 0) {
    // Send the release signal to ATtiny (actually release the parachute!)
    //digitalWrite(VX_PARA_PIN_RELEASE, HIGH);
    vx_para_RELEASING = true;
    //vx_para_release_timer = millis();
    vx_teleSendWarning(VX_TELE_WARNING_RELEASED);
  }
}


// Checks auto release parameters
void vx_para_check() {
  long current_time = millis();
  
  /*Serial.print("DEBUG: vx_para_check(): current_time = ");
  Serial.print(current_time);
  Serial.print(", (current_time - vx_g_pos_last_gps_fix_time) = "); 
  Serial.print(current_time - vx_g_pos_last_gps_fix_time);
  Serial.print(", VX_PARA_NOGPSFIX_TRIGGER_TIME = ");
  Serial.print(VX_PARA_NOGPSFIX_TRIGGER_TIME);
  Serial.print(", check GPS fix time = "); 
  Serial.println((current_time - vx_g_pos_last_gps_fix_time) > VX_PARA_NOGPSFIX_TRIGGER_TIME);*/
  
  
  // 1. Check GPS fix time
  if((current_time - vx_g_pos_last_gps_fix_time) > VX_PARA_NOGPSFIX_TRIGGER_TIME) {
    //Serial.println("DEBUG: vx_g_para_check(): NOGPSFIX (time limit)");
    
    // 1a. Check if no connection to ground station (GS)
    if((current_time - vx_g_tele_last_received_time) > VX_PARA_NOGROUNDSTATION_TRIGGER_TIME) {
      //Serial.println("DEBUG: vx_g_para_check(): NOGPSFIX (time limit) + NO_GS");
      
      // 1b. Check if no connection to radio control (RC)
      if(!(vx_g_flightphase == 1 || vx_g_flightphase == 2)) {
        //Serial.println("DEBUG: vx_g_para_check(): NOGPSFIX (time limit) + NO_RC");
        
        // 1c. No GPS fix, NO contact with GS, NO contact with RS --> RELEASE PARACHUTE!
        vx_para_auto_release(VX_PARA_RELEASE_WARNING_TIME__NOGPSFIX, VX_PARA_AUTO_RELEASE__NOGPSFIX);
      }
      else { // 1b. Got contact with RS
        if(vx_para_auto_releasing__nogpsfix) { // If releasing, cancel
          vx_para_auto_cancel(VX_PARA_AUTO_CANCEL__NOGPSFIX_RC);
        }
      }
    }
    else { // 1a. Got contact with GS
      if(vx_para_auto_releasing__nogpsfix) { // If releasing, cancel
        vx_para_auto_cancel(VX_PARA_AUTO_CANCEL__NOGPSFIX_GS);
      }
    }
  }
  else { // 1. Got GPS fix
    if(vx_para_auto_releasing__nogpsfix) { // If releasing, cancel
      vx_para_auto_cancel(VX_PARA_AUTO_CANCEL__NOGPSFIX_GPSFIX);
    }
  }
  
  // 2. Check Boundary Box
  if(vx_para_boundary_box_violation()) {
    
    // 2a. Check altitude under 3km
    if(vx_g_current_pos.alt < 300000) { // Altitude below 3km
      // 2b. OUTSIDE boundary box, BELOW 3km --> RELEASE PARACHUTE! in 30 seconds
      vx_para_auto_release(VX_PARA_RELEASE_WARNING_TIME__BOUNDARYBOX_BELOW_3KM, VX_PARA_AUTO_RELEASE__BOUNDARYBOX);
    }
    else { // Altitude above 3km
      // 2b. OUTSIDE boundary box, ABOVE 3km --> RELEASE PARACHUTE! in 60 seconds
      vx_para_auto_release(VX_PARA_RELEASE_WARNING_TIME__BOUNDARYBOX_ABOVE_3KM, VX_PARA_AUTO_RELEASE__BOUNDARYBOX);
    }
  } 
  else { // 2. Back into box
    if(vx_para_auto_releasing__boundarybox) { // If releasing, cancel
      vx_para_auto_cancel(VX_PARA_AUTO_CANCEL__BOUNDARYBOX);
    }
  }
  
  // 3. Check Altitude limit
  if((vx_g_current_pos.alt < VX_PARA_MIN_ALTITUDE) && (vx_g_para_ascent == false)) {
    Serial.println("DEBUG: vx_g_para_check(): LOWALT");
    
    // 3a. Check if no connection to ground station (GS)
    if((current_time - vx_g_tele_last_received_time) > VX_PARA_NOGROUNDSTATION_TRIGGER_TIME) {
      Serial.println("DEBUG: vx_g_para_check(): LOWALT + NO_GS");
      
      // 3b. Check if no connection to radio control (RC)
      if(!(vx_g_flightphase == 1 || vx_g_flightphase == 2)) {
        Serial.println("DEBUG: vx_g_para_check(): LOWALT + NO_RC");
        
        // 3c. Too low altitude, NO contact with GS, NO contact with RS --> RELEASE PARACHUTE!
        vx_para_auto_release(VX_PARA_RELEASE_WARNING_TIME__LOWALTITUDE, VX_PARA_AUTO_RELEASE__LOWALTITUDE);
      }
      else { // 1b. Got contact with RS
        if(vx_para_auto_releasing__nogpsfix) { // If releasing, cancel
          vx_para_auto_cancel(VX_PARA_AUTO_CANCEL__LOWALTITUDE_RC);
        }
      }
    }
    else { // 1a. Got contact with GS
      if(vx_para_auto_releasing__nogpsfix) { // If releasing, cancel
        vx_para_auto_cancel(VX_PARA_AUTO_CANCEL__LOWALTITUDE_GS);
      }
    }
  }
  //else { 
      // Note: Doesn't care if we are above 1km again, still releases
    //}
  //}
}


// (Direct) Manual release of parachute (triggered from telecommando)
void vx_para_manual_release() {
  // Send the release signal to ATtiny (actually release the parachute!)
  vx_para_RELEASING = true;
  
  // Send warning message
  vx_teleSendWarning(VX_TELE_WARNING_RELEASED); 
}

// Set a countdown time and start the countdown
void vx_para_manual_delay(long delay_time) {
  vx_para_counting_down = true; // Start countdown
  vx_g_para_countdown_clock = delay_time; // Set countdown time
  vx_para_auto_releasing__telecommand = true;
  
  vx_teleSendHousekeeping();
  
  /*Serial.print("DEBUG: vx_para_manual_delay(): delay_time = "); 
  Serial.print(delay_time); 
  Serial.print(", vx_g_para_countdown_clock = "); 
  Serial.println(vx_g_para_countdown_clock); */
}

// Manual cancel of (automatic) parachute release
void vx_para_manual_cancel() {
  vx_para_counting_down = false; // Stop the countdown
  vx_g_para_countdown_clock = -1; // Set the timer to show no countdown: -1
  vx_para_auto_releasing__telecommand = false;
  
  vx_para_RELEASING = false;
  
  vx_teleSendHousekeeping();
  vx_teleSendWarning(VX_TELE_WARNING_CANCELED);
}

void vx_para_manual_ascent_on() {
  vx_g_para_ascent = true;
}

void vx_para_manual_ascent_off() {
  vx_g_para_ascent = false;
}

/*void vx_para_manual_reset() {
  vx_para_manual_cancel();
  //digitalWrite(VX_PARA_PIN_RELEASE, LOW);
  vx_para_RELEASING = false;
}*/

// ************************ PRIVATE FUNCTIONS **************************

// Automatic release of parachute, after a set "warning time", with a specified reason.
void vx_para_auto_release(long countdown_time, int reason) {
  
  // NO AUTOMATIC RELEASE IF CLOSE TO HOMEBASE (within "safety box")
  if(vx_para_is_close_to_homebase() == false) {
    vx_para_counting_down = true;
    // Only set new countdown time if smaller than the already existing (in case it is already counting down)
    if((vx_g_para_countdown_clock > countdown_time) || (vx_g_para_countdown_clock == -1)) {
      vx_g_para_countdown_clock = countdown_time;
    }
    
    vx_teleSendHousekeeping();
  }
  else {
    /*Serial3.print("DEBUG: vx_para_auto_release(): reason: ");
    Serial3.print(reason);
    Serial3.println(" (in safety box).");*/
  }
  
  // Send warning message and set correct boolean to true
  switch(reason) {
    case VX_PARA_AUTO_RELEASE__NOGPSFIX:
      // Only send warning message once
      if(vx_para_auto_releasing__nogpsfix == false) vx_teleSendWarning(VX_TELE_WARNING_PARACHUTE__NOGPSFIX);
      vx_para_auto_releasing__nogpsfix = true;
      break;
      
    case VX_PARA_AUTO_RELEASE__BOUNDARYBOX:
      // Only send warning message once
      if(vx_para_auto_releasing__boundarybox == false) vx_teleSendWarning(VX_TELE_WARNING_PARACHUTE__BOUNDARYBOX);
      vx_para_auto_releasing__boundarybox = true;
      break;
      
    case VX_PARA_AUTO_RELEASE__LOWALTITUDE:
      // Only send warning message once
      if(vx_para_auto_releasing__lowaltitude == false) vx_teleSendWarning(VX_TELE_WARNING_PARACHUTE__LOWALTITUDE);
      vx_para_auto_releasing__lowaltitude = true;
      break;
      
    default:
      // ...
      break;
  }
  
}

// Automatic canclation of (automatic) parachute release 
void vx_para_auto_cancel(int reason) {
  // Set correct boolean to false (and send warning message)
  switch(reason) {
    case VX_PARA_AUTO_CANCEL__NOGPSFIX_GPSFIX:
      if(vx_para_auto_releasing__nogpsfix == true) vx_teleSendWarning(VX_TELE_WARNING_CANCELED__NOGPSFIX_GPSFIX);
      vx_para_auto_releasing__nogpsfix = false;
      break;
    case VX_PARA_AUTO_CANCEL__NOGPSFIX_GS:
      if(vx_para_auto_releasing__nogpsfix == true) vx_teleSendWarning(VX_TELE_WARNING_CANCELED__NOGPSFIX_GS);
      vx_para_auto_releasing__nogpsfix = false;
      break;
    case VX_PARA_AUTO_CANCEL__NOGPSFIX_RC:
      if(vx_para_auto_releasing__nogpsfix == true) vx_teleSendWarning(VX_TELE_WARNING_CANCELED__NOGPSFIX_RC);
      vx_para_auto_releasing__nogpsfix = false;
      break;
      
    case VX_PARA_AUTO_CANCEL__BOUNDARYBOX:
      if(vx_para_auto_releasing__boundarybox == true) vx_teleSendWarning(VX_TELE_WARNING_CANCELED__BOUNDARYBOX);
      vx_para_auto_releasing__boundarybox = false;
      break;
      
    case VX_PARA_AUTO_CANCEL__LOWALTITUDE_RC:
      if(vx_para_auto_releasing__lowaltitude == true) vx_teleSendWarning(VX_TELE_WARNING_CANCELED__LOWALTITUDE_RC);
      vx_para_auto_releasing__lowaltitude = false;
      break;
    case VX_PARA_AUTO_CANCEL__LOWALTITUDE_GS:
      if(vx_para_auto_releasing__lowaltitude == true) vx_teleSendWarning(VX_TELE_WARNING_CANCELED__LOWALTITUDE_GS);
      vx_para_auto_releasing__lowaltitude = false;
      break;
      
    default:
      // ...
      break;
  }
  
  // If all reasons have been set to false --> cancel the countdown
  if((vx_para_auto_releasing__nogpsfix == false) && 
  (vx_para_auto_releasing__boundarybox == false) && 
  (vx_para_auto_releasing__lowaltitude == false) && 
  (vx_para_auto_releasing__telecommand == false)) {
    vx_para_counting_down = false;
    vx_g_para_countdown_clock = -1;
    vx_para_RELEASING = false;
    //Serial3.println("DEBUG: vx_para_auto_cancel(): resetting countdown"); 
    vx_teleSendHousekeeping();
  }
}


// ************************************ DISTANCE CHECK FUNCTIONS *************************************

// Check if plane is outside of boundary box
boolean vx_para_boundary_box_violation() {
  //long homebase_dist = vx_calculate_wp_distance(g_gps->latitude, g_gps->longitude, vx_g_home.lat, vx_g_home.lon);
  //long homebase_dist = vx_calculate_wp_distance(vx_g_current_pos.lat, vx_g_current_pos.lon, vx_g_home.lat, vx_g_home.lon);
  long homebase_dist = vx_calculate_wp_distance(vx_g_current_pos.lat, vx_g_current_pos.lon, vx_g_center_circle.lat, vx_g_center_circle.lon);
  
  if(homebase_dist > VX_PARA_BOUNDARYBOX_DISTANCE) {
    return true;
  }
  else {
    return false;
  }

}

// Check if plane is close enough to cancel all parachute releases
boolean vx_para_is_close_to_homebase() {
  //long homebase_dist = vx_calculate_wp_distance(g_gps->latitude, g_gps->longitude, vx_g_home.lat, vx_g_home.lon);
  long homebase_dist = vx_calculate_wp_distance(vx_g_current_pos.lat, vx_g_current_pos.lon, vx_g_home.lat, vx_g_home.lon);
  
  if(homebase_dist < VX_PARA_SAFETYBOX_DISTANCE) {
    return true;
  }
  else {
    return false;
  }
}

